#ifndef MOTION_MANAGER_HPP
#define MOTION_MANAGER_HPP

#include <mqtt/async_client.h>

namespace mqtt2ros {

class MqttInterface : public mqtt::callback
{
  public:

    using Callback_t = std::function<void(const mqtt::const_message_ptr&)>;
    typedef struct {
      std::string broker;
      std::string client_id;
      std::string topic;
      Callback_t callback = nullptr;
    } MqttConfigure_t;


    MqttInterface(MqttConfigure_t config) : config_(config) {
        mqtt_client_ = std::make_unique<mqtt::async_client>(config_.broker, config_.client_id);
        mqtt_client_->set_callback(*this);
    }

    bool connect() {
      try {
        mqtt::connect_options connOpts;
        connOpts.set_clean_session(true);
        mqtt_client_->connect(connOpts)->wait();
        mqtt_client_->subscribe(config_.topic, 1)->wait();
        return true;
      } catch (const mqtt::exception& e) {
        return false;
      }

    }
    void disconnect() {
      if (mqtt_client_ && mqtt_client_->is_connected()) {
        mqtt_client_->disconnect()->wait();
      }
    }

    ~MqttInterface() {
      this->disconnect();
    }

    void message_arrived(mqtt::const_message_ptr msg) override {
      if (config_.callback) {
        config_.callback(msg);
      } else {
        std::cerr << "No callback set for message: " << msg->get_topic() << std::endl;
      }
    }

  private:
  std::unique_ptr<mqtt::async_client> mqtt_client_;
  MqttConfigure_t config_;

};





} // namespace mqtt2ros

#endif //MOTION_MANAGER_HPP
